package com.atguigu.schedule.service.impl;

import com.atguigu.schedule.common.Result;
import com.atguigu.schedule.mapper.SlamSessionMapper;
import com.atguigu.schedule.pojo.SlamSession;
import com.atguigu.schedule.service.RealSlamProcessingService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
import java.time.LocalDateTime;
import java.util.*;
import java.util.concurrent.CompletableFuture;
import java.util.concurrent.TimeUnit;

/**
 * 真实SLAM处理服务实现
 * 参考人脸识别功能的实现模式
 */
@Slf4j
@Service
public class RealSlamProcessingServiceImpl implements RealSlamProcessingService {
    
    private final SlamSessionMapper slamSessionMapper;
    
    public RealSlamProcessingServiceImpl(SlamSessionMapper slamSessionMapper) {
        this.slamSessionMapper = slamSessionMapper;
    }
    
    // 处理超时时间（秒）
    private static final int PROCESS_TIMEOUT_SECONDS = 30;
    
    // Python脚本路径 - 使用增强版SLAM处理器
    private String pythonScriptPath = "/opt/smart-care-car/enhanced_slam_processor.py";
    
    // 当前处理的会话
    private Map<Long, SlamProcessingSession> activeSessions = new HashMap<>();
    
    @Override
    public Result<Map<String, Object>> processVideoFrame(Long sessionId, byte[] imageData) {
        try {
            log.info("🎯 开始处理SLAM视频帧，会话ID: {}", sessionId);
            
            // 检查会话是否存在，如果不存在则自动创建
            SlamSession session = slamSessionMapper.selectById(sessionId);
            if (session == null) {
                log.info("🔧 会话不存在，自动创建: {}", sessionId);
                // 自动创建会话
                session = new SlamSession();
                session.setId(sessionId);
                session.setSessionName("AutoCreated_" + sessionId);
                session.setStatus(SlamSession.SessionStatus.RUNNING);
                session.setStartedAt(LocalDateTime.now());
                session.setCreatedAt(LocalDateTime.now());
                session.setCurrentFrame(0);
                session.setProgressPercentage(0.0);
                
                // 初始化SLAM数据
                Map<String, Object> initialData = new HashMap<>();
                initialData.put("keyframes", 0);
                initialData.put("mapPoints", 0);
                initialData.put("trajectoryLength", 0.0);
                initialData.put("timestamp", LocalDateTime.now());
                
                try {
                    String jsonData = new com.fasterxml.jackson.databind.ObjectMapper().writeValueAsString(initialData);
                    session.setRealtimeTrajectory(jsonData);
                } catch (Exception e) {
                    log.warn("⚠️ 初始化SLAM数据失败: {}", e.getMessage());
                }
                
                slamSessionMapper.insert(session);
                log.info("✅ 会话已自动创建: {}", sessionId);
            }
            
            // 创建临时图像文件
            String tempImagePath = "/tmp/slam_frame_" + sessionId + "_" + System.currentTimeMillis() + ".jpg";
            Files.write(Paths.get(tempImagePath), imageData);
            log.info("📁 创建临时图像文件: {}", tempImagePath);
            
            // 异步调用完整真实SLAM处理器
            CompletableFuture<SlamProcessResult> future = executeCompleteRealSlamProcessing(sessionId, tempImagePath);
            
            try {
                SlamProcessResult result = future.get(PROCESS_TIMEOUT_SECONDS, TimeUnit.SECONDS);
                
                if (result.isSuccess()) {
                    log.info("✅ SLAM处理成功，会话ID: {}", sessionId);
                    
                    // 更新会话数据
                    updateSessionData(session, result);
                    
                    // 返回处理结果 - 增强版SLAM数据
                    Map<String, Object> responseData = new HashMap<>();
                    responseData.put("sessionId", sessionId);
                    responseData.put("keyframes", result.getKeyframes());
                    responseData.put("mapPoints", result.getMapPoints());
                    responseData.put("trajectoryLength", result.getTrajectoryLength());
                    responseData.put("progress", result.getProgress());
                    responseData.put("status", "PROCESSING");
                    responseData.put("timestamp", LocalDateTime.now());
                    
                    // 增强版SLAM特有数据
                    responseData.put("algorithm", "Enhanced SLAM with 3D Trajectory");
                    responseData.put("trajectoryPoints", result.getTrajectoryPoints());
                    responseData.put("poses", result.getPoses());
                    responseData.put("has3DCoordinates", true);
                    
                    return Result.ok(responseData);
                    
                } else {
                    log.error("❌ SLAM处理失败，会话ID: {}, 错误: {}", sessionId, result.getError());
                    return Result.build(null, 500, "SLAM处理失败: " + result.getError());
                }
                
            } catch (Exception e) {
                log.error("❌ SLAM处理超时或异常，会话ID: {}", sessionId, e);
                return Result.build(null, 500, "SLAM处理超时: " + e.getMessage());
            } finally {
                // 清理临时文件
                try {
                    Files.deleteIfExists(Paths.get(tempImagePath));
                } catch (IOException e) {
                    log.warn("⚠️ 清理临时文件失败: {}", tempImagePath);
                }
            }
            
        } catch (Exception e) {
            log.error("❌ 处理SLAM视频帧失败", e);
            return Result.build(null, 500, "处理视频帧失败: " + e.getMessage());
        }
    }
    
    @Override
    public Result<String> startSlamSession(Long sessionId, String sessionName) {
        try {
            log.info("🚀 启动完整真实SLAM会话: {}", sessionName);
            
            // 调用Python脚本启动SLAM会话
            ProcessBuilder pb = new ProcessBuilder(
                "python3", pythonScriptPath,
                "--session-id", sessionId.toString(),
                "--output-dir", "/opt/smart-care-car/slam/output",
                "--action", "start"
            );
            
            Process process = pb.start();
            int exitCode = process.waitFor();
            
            if (exitCode == 0) {
                // 创建或更新会话
                SlamSession session = slamSessionMapper.selectById(sessionId);
                if (session == null) {
                    session = new SlamSession();
                }
                session.setId(sessionId);
                session.setSessionName(sessionName);
                session.setStatus(SlamSession.SessionStatus.RUNNING);
                session.setStartedAt(LocalDateTime.now());
                session.setCurrentFrame(0);
                
                // 初始化SLAM数据
                Map<String, Object> initialData = new HashMap<>();
                initialData.put("keyframes", 0);
                initialData.put("mapPoints", 0);
                initialData.put("trajectoryLength", 0.0);
                initialData.put("timestamp", LocalDateTime.now());
                
                try {
                    String jsonData = new com.fasterxml.jackson.databind.ObjectMapper().writeValueAsString(initialData);
                    session.setRealtimeTrajectory(jsonData);
                } catch (Exception e) {
                    log.warn("⚠️ 初始化SLAM数据失败: {}", e.getMessage());
                }
                
                session.setProgressPercentage(0.0);
            
            slamSessionMapper.insert(session);
            
            // 创建处理会话
            SlamProcessingSession processingSession = new SlamProcessingSession();
            processingSession.setSessionId(sessionId);
            processingSession.setSessionName(sessionName);
            processingSession.setStartTime(LocalDateTime.now());
            processingSession.setStatus("RUNNING");
            
            activeSessions.put(sessionId, processingSession);
            
                log.info("✅ 真实SLAM会话已启动，会话ID: {}", sessionId);
                return Result.ok("SLAM会话启动成功");
            } else {
                log.error("❌ 启动完整真实SLAM会话失败，退出码: {}", exitCode);
                return Result.build(null, 500, "启动SLAM会话失败");
            }
            
        } catch (Exception e) {
            log.error("❌ 启动SLAM会话失败", e);
            return Result.build(null, 500, "启动SLAM会话失败: " + e.getMessage());
        }
    }
    
    @Override
    public Result<String> stopSlamSession(Long sessionId) {
        try {
            log.info("🛑 停止真实SLAM会话: {}", sessionId);
            
            // 更新会话状态
            SlamSession session = slamSessionMapper.selectById(sessionId);
            if (session != null) {
                session.setStatus(SlamSession.SessionStatus.STOPPED);
                session.setEndedAt(LocalDateTime.now());
                slamSessionMapper.insert(session);
            }
            
            // 移除处理会话
            activeSessions.remove(sessionId);
            
            log.info("✅ 真实SLAM会话已停止，会话ID: {}", sessionId);
            return Result.ok("SLAM会话停止成功");
            
        } catch (Exception e) {
            log.error("❌ 停止SLAM会话失败", e);
            return Result.build(null, 500, "停止SLAM会话失败: " + e.getMessage());
        }
    }
    
    @Override
    public Result<Map<String, Object>> getSlamStatus(Long sessionId) {
        try {
            SlamSession session = slamSessionMapper.selectById(sessionId);
            if (session == null) {
                return Result.build(null, 404, "SLAM会话不存在");
            }
            
            SlamProcessingSession processingSession = activeSessions.get(sessionId);
            
            Map<String, Object> status = new HashMap<>();
            status.put("sessionId", sessionId);
            status.put("sessionName", session.getSessionName());
            status.put("status", session.getStatus().toString());
            // 从realtimeTrajectory字段解析SLAM数据
            Map<String, Object> slamData = parseSlamData(session.getRealtimeTrajectory());
            status.put("keyframes", slamData.getOrDefault("keyframes", 0));
            status.put("mapPoints", slamData.getOrDefault("mapPoints", 0));
            status.put("trajectoryLength", slamData.getOrDefault("trajectoryLength", 0.0));
            status.put("progress", session.getProgressPercentage());
            status.put("currentFrame", session.getCurrentFrame());
            status.put("isProcessing", processingSession != null && "RUNNING".equals(processingSession.getStatus()));
            status.put("timestamp", LocalDateTime.now());
            
            return Result.ok(status);
            
        } catch (Exception e) {
            log.error("❌ 获取SLAM状态失败", e);
            return Result.build(null, 500, "获取SLAM状态失败: " + e.getMessage());
        }
    }
    
    /**
     * 异步执行SLAM处理
     */
    private CompletableFuture<SlamProcessResult> executeCompleteRealSlamProcessing(Long sessionId, String imagePath) {
        return CompletableFuture.supplyAsync(() -> {
            try {
                log.info("🐍 执行完整真实SLAM处理脚本: {}", pythonScriptPath);
                
                ProcessBuilder pb = new ProcessBuilder(
                    "python3", pythonScriptPath,
                    "--session-id", sessionId.toString(),
                    "--image-path", imagePath,
                    "--output-dir", "/opt/smart-care-car/slam/output",
                    "--action", "process"
                );
                
                pb.redirectErrorStream(true);
                Process process = pb.start();
                
                // 读取输出
                StringBuilder output = new StringBuilder();
                try (Scanner scanner = new Scanner(process.getInputStream())) {
                    while (scanner.hasNextLine()) {
                        String line = scanner.nextLine();
                        output.append(line).append("\n");
                        log.debug("Python输出: {}", line);
                    }
                }
                
                int exitCode = process.waitFor();
                String outputStr = output.toString();
                
                log.info("🐍 Python脚本执行完成，退出码: {}, 输出长度: {}", exitCode, outputStr.length());
                
                if (exitCode == 0) {
                    // 解析输出结果
                    return parseSlamResult(outputStr);
                } else {
                    return SlamProcessResult.error("Python脚本执行失败，退出码: " + exitCode);
                }
                
            } catch (Exception e) {
                log.error("❌ 执行SLAM处理脚本失败", e);
                return SlamProcessResult.error("执行脚本失败: " + e.getMessage());
            }
        });
    }
    
    /**
     * 解析SLAM处理结果
     */
    private SlamProcessResult parseSlamResult(String output) {
        try {
            // 解析Python脚本的输出
            // 期望格式: KEYFRAMES:10,MAPPOINTS:150,TRAJECTORY:5.5,PROGRESS:2.5
            String[] lines = output.split("\n");
            int keyframes = 0;
            int mapPoints = 0;
            double trajectoryLength = 0.0;
            double progress = 0.0;
            
            for (String line : lines) {
                if (line.startsWith("KEYFRAMES:")) {
                    keyframes = Integer.parseInt(line.substring(10));
                } else if (line.startsWith("MAPPOINTS:")) {
                    mapPoints = Integer.parseInt(line.substring(10));
                } else if (line.startsWith("TRAJECTORY:")) {
                    trajectoryLength = Double.parseDouble(line.substring(11));
                } else if (line.startsWith("PROGRESS:")) {
                    progress = Double.parseDouble(line.substring(9));
                }
            }
            
            return SlamProcessResult.success(keyframes, mapPoints, trajectoryLength, progress);
            
        } catch (Exception e) {
            log.error("❌ 解析SLAM结果失败", e);
            return SlamProcessResult.error("解析结果失败: " + e.getMessage());
        }
    }
    
    /**
     * 更新会话数据
     */
    private void updateSessionData(SlamSession session, SlamProcessResult result) {
        session.setCurrentFrame(session.getCurrentFrame() + 1);
            // 将SLAM数据存储到realtimeTrajectory字段中
            Map<String, Object> slamData = new HashMap<>();
            slamData.put("keyframes", result.getKeyframes());
            slamData.put("mapPoints", result.getMapPoints());
            slamData.put("trajectoryLength", result.getTrajectoryLength());
            slamData.put("timestamp", LocalDateTime.now());
            
            try {
                String jsonData = new com.fasterxml.jackson.databind.ObjectMapper().writeValueAsString(slamData);
                session.setRealtimeTrajectory(jsonData);
            } catch (Exception e) {
                log.warn("⚠️ 序列化SLAM数据失败: {}", e.getMessage());
            }
        session.setProgressPercentage(result.getProgress());
        
        slamSessionMapper.insert(session);
        
        log.info("📊 更新会话数据: 关键帧={}, 地图点={}, 轨迹长度={}, 进度={}%", 
            result.getKeyframes(), result.getMapPoints(), result.getTrajectoryLength(), result.getProgress());
    }
    
    /**
     * 解析SLAM数据
     */
    private Map<String, Object> parseSlamData(String jsonData) {
        Map<String, Object> defaultData = new HashMap<>();
        defaultData.put("keyframes", 0);
        defaultData.put("mapPoints", 0);
        defaultData.put("trajectoryLength", 0.0);
        defaultData.put("algorithm", "Enhanced SLAM with 3D Trajectory");
        defaultData.put("has3DCoordinates", false);
        
        if (jsonData == null || jsonData.trim().isEmpty()) {
            return defaultData;
        }
        
        try {
            com.fasterxml.jackson.databind.ObjectMapper mapper = new com.fasterxml.jackson.databind.ObjectMapper();
            @SuppressWarnings("unchecked")
            Map<String, Object> data = mapper.readValue(jsonData, Map.class);
            
            // 确保必要字段存在
            data.putIfAbsent("keyframes", 0);
            data.putIfAbsent("mapPoints", 0);
            data.putIfAbsent("trajectoryLength", 0.0);
            data.putIfAbsent("algorithm", "Enhanced SLAM with 3D Trajectory");
            data.putIfAbsent("has3DCoordinates", true);
            
            // 解析3D轨迹数据
            if (data.containsKey("trajectory_points")) {
                @SuppressWarnings("unchecked")
                List<Map<String, Object>> trajectoryPoints = (List<Map<String, Object>>) data.get("trajectory_points");
                data.put("trajectoryPointsCount", trajectoryPoints.size());
                
                // 计算轨迹统计
                if (trajectoryPoints.size() > 1) {
                    double totalLength = 0.0;
                    for (int i = 1; i < trajectoryPoints.size(); i++) {
                        Map<String, Object> p1 = trajectoryPoints.get(i-1);
                        Map<String, Object> p2 = trajectoryPoints.get(i);
                        
                        if (p1.containsKey("position_3d") && p2.containsKey("position_3d")) {
                            @SuppressWarnings("unchecked")
                            List<Double> pos1 = (List<Double>) p1.get("position_3d");
                            @SuppressWarnings("unchecked")
                            List<Double> pos2 = (List<Double>) p2.get("position_3d");
                            
                            if (pos1.size() >= 3 && pos2.size() >= 3) {
                                double dx = pos2.get(0) - pos1.get(0);
                                double dy = pos2.get(1) - pos1.get(1);
                                double dz = pos2.get(2) - pos1.get(2);
                                double distance = Math.sqrt(dx*dx + dy*dy + dz*dz);
                                totalLength += distance;
                            }
                        }
                    }
                    data.put("calculatedTrajectoryLength", totalLength);
                }
            }
            
            return data;
        } catch (Exception e) {
            log.warn("⚠️ 解析增强版SLAM数据失败: {}", e.getMessage());
            return defaultData;
        }
    }
    
    /**
     * SLAM处理结果类 - 增强版支持3D轨迹数据
     */
    public static class SlamProcessResult {
        private boolean success;
        private int keyframes;
        private int mapPoints;
        private double trajectoryLength;
        private double progress;
        private String error;
        
        // 增强版SLAM数据
        private List<Map<String, Object>> trajectoryPoints;
        private List<Map<String, Object>> poses;
        private String algorithm;
        
        public static SlamProcessResult success(int keyframes, int mapPoints, double trajectoryLength, double progress) {
            SlamProcessResult result = new SlamProcessResult();
            result.success = true;
            result.keyframes = keyframes;
            result.mapPoints = mapPoints;
            result.trajectoryLength = trajectoryLength;
            result.progress = progress;
            result.algorithm = "Enhanced SLAM with 3D Trajectory";
            result.trajectoryPoints = new ArrayList<>();
            result.poses = new ArrayList<>();
            return result;
        }
        
        public static SlamProcessResult success(int keyframes, int mapPoints, double trajectoryLength, double progress, 
                                               List<Map<String, Object>> trajectoryPoints, List<Map<String, Object>> poses) {
            SlamProcessResult result = new SlamProcessResult();
            result.success = true;
            result.keyframes = keyframes;
            result.mapPoints = mapPoints;
            result.trajectoryLength = trajectoryLength;
            result.progress = progress;
            result.algorithm = "Enhanced SLAM with 3D Trajectory";
            result.trajectoryPoints = trajectoryPoints != null ? trajectoryPoints : new ArrayList<>();
            result.poses = poses != null ? poses : new ArrayList<>();
            return result;
        }
        
        public static SlamProcessResult error(String error) {
            SlamProcessResult result = new SlamProcessResult();
            result.success = false;
            result.error = error;
            return result;
        }
        
        // Getters
        public boolean isSuccess() { return success; }
        public int getKeyframes() { return keyframes; }
        public int getMapPoints() { return mapPoints; }
        public double getTrajectoryLength() { return trajectoryLength; }
        public double getProgress() { return progress; }
        public String getError() { return error; }
        public List<Map<String, Object>> getTrajectoryPoints() { return trajectoryPoints; }
        public List<Map<String, Object>> getPoses() { return poses; }
        public String getAlgorithm() { return algorithm; }
    }
    
    /**
     * SLAM处理会话类
     */
    public static class SlamProcessingSession {
        private Long sessionId;
        private String sessionName;
        private LocalDateTime startTime;
        private String status;
        
        // Getters and Setters
        public Long getSessionId() { return sessionId; }
        public void setSessionId(Long sessionId) { this.sessionId = sessionId; }
        
        public String getSessionName() { return sessionName; }
        public void setSessionName(String sessionName) { this.sessionName = sessionName; }
        
        public LocalDateTime getStartTime() { return startTime; }
        public void setStartTime(LocalDateTime startTime) { this.startTime = startTime; }
        
        public String getStatus() { return status; }
        public void setStatus(String status) { this.status = status; }
    }
}
